/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, 2013, Willow Garage, Inc.
*  Copyright (c) 2015, Fetch Robotics, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*         David V. Lu!!
*********************************************************************/
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;

namespace costmap_2d
{

StaticLayer::StaticLayer() : dsrv_(NULL) {}

StaticLayer::~StaticLayer() { delete dsrv_; }

void StaticLayer::onInitialize()
{
    ros::NodeHandle nh("~/" + name_), g_nh;
    current_ = true;
    // global_frame_在global_costmap时为map, local_costmap时为odom_相关的话题
    global_frame_ = layered_costmap_->getGlobalFrameID();
    std::string map_topic;
    nh.param("map_topic", map_topic, std::string("map"));
    // 仅订阅地图话题上的第一条消息，忽略所有后续消息。
    nh.param("first_map_only", first_map_only_, false);
    // 除map_topic外，还订阅map_topic + "_updates"。
    nh.param("subscribe_to_updates", subscribe_to_updates_, false);
    // 如果为true，则地图消息中的未知值将直接转换到图层。否则，地图消息中的未知值将转换为层中的空闲空间。
    nh.param("track_unknown_space", track_unknown_space_, true);
    // 只有当静态层不是底层时才重要。如果为true，则仅将最大值写入主代价地图。
    nh.param("use_maximum", use_maximum_, false);
    int temp_lethal_threshold, temp_unknown_cost_value;
    // 致命的代价阈值，默认100
    nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
    // 未知区域的代价值为-1
    nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
    // 如果为true，则将所有地图消息值转换为NO_INFORMATION/FREE_SPACE/LETHAL_OBSTACLE（三个值）。如果为false，则可能出现完整的中间值范围。
    nh.param("trinary_costmap", trinary_costmap_, true);
    lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
    unknown_cost_value_ = temp_unknown_cost_value;
    // Only resubscribe if topic has changed
    if (map_sub_.getTopic() != ros::names::resolve(map_topic))
    {
        // 从map server的发出话题中订阅地图数据
        // we'll subscribe to the latched topic that the map server uses
        ROS_INFO("Requesting the map...");
        map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
        map_received_ = false;
        has_updated_data_ = false;
        ros::Rate r(10);
        // 一直阻塞，直到将map放入到代价地图后
        while (!map_received_ && g_nh.ok())
        {
            ros::spinOnce();
            r.sleep();
        }
        ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
        if (subscribe_to_updates_)
        {
            ROS_INFO("Subscribing to updates");
            map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);

        }
    }
    else has_updated_data_ = true;
    delete dsrv_;
    dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
    dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb =
            [this](auto &config, auto level) { reconfigureCB(config, level); };
    dsrv_->setCallback(cb);
}

void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
    if (config.enabled != enabled_)
    {
        enabled_ = config.enabled;
        has_updated_data_ = true;
        x_ = y_ = 0;
        width_ = size_x_;
        height_ = size_y_;
    }
}

void StaticLayer::matchSize()
{
    // If we are using rolling costmap, the static map size is unrelated to the size of the layered costmap
    // 如果我们使用滚动costmap，则静态地图大小与分层costmap的大小无关
    // 如果没有在滚动，创建一个对象master
    if (!layered_costmap_->isRolling())
    {
        Costmap2D *master = layered_costmap_->getCostmap();
        resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
                  master->getOriginX(), master->getOriginY());
    }
}

unsigned char StaticLayer::interpretValue(unsigned char value)
{
    // 1. 如果阈值lethal_threshold视为障碍物
    // 2. 没有信息的就认为是未探测的区域
    // 3. 否则为自由空间，true, -1
    // check if the static value is above the unknown or lethal thresholds
    // 检查静态值是否高于未知或致命阈值 map数据为-1 时会进入这里
#ifdef SEMANTIC
    if (track_unknown_space_ && value == unknown_cost_value_) return toOri(XJU_COST_NO_INFORMATION, XJU_OPTION_INIT);
    else if (!track_unknown_space_ && value == unknown_cost_value_) return toOri(XJU_COST_FREE_SPACE, XJU_OPTION_INIT);
    else if (value >= lethal_threshold_) return toOri(XJU_COST_LETHAL_OBSTACLE, XJU_OPTION_INIT);
    else if (trinary_costmap_) return toOri(XJU_COST_FREE_SPACE, XJU_OPTION_INIT);
    // map数据为0 时进入这里
    double scale = (double) value / lethal_threshold_;
    return toOri(scale * XJU_COST_LETHAL_OBSTACLE, XJU_OPTION_INIT);
#else
    if (track_unknown_space_ && value == unknown_cost_value_) return NO_INFORMATION;
    else if (!track_unknown_space_ && value == unknown_cost_value_) return FREE_SPACE;
        // map数据为100 时，>=100 就是致命的障碍物，会进入这里
    else if (value >= lethal_threshold_) return LETHAL_OBSTACLE;
    else if (trinary_costmap_) return FREE_SPACE;
    // map数据为0 时进入这里
    double scale = (double) value / lethal_threshold_;
    return scale * LETHAL_OBSTACLE;
#endif
}

void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map)
{
    // step 1 从地图消息中获取地图的宽、高和分辨率
    unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
    ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
    // step 2 判断master costmap地图是否发生变化，有则改变尺寸
    // resize costmap if size, resolution or origin do not match
    // 方法一: 如果master map的尺寸、分辨率或原点与获取到的地图不匹配，重新设置master map
    Costmap2D *master = layered_costmap_->getCostmap();
    if (!layered_costmap_->isRolling() &&
        (master->getSizeInCellsX() != size_x ||
         master->getSizeInCellsY() != size_y ||
         master->getResolution() != new_map->info.resolution ||
         master->getOriginX() != new_map->info.origin.position.x ||
         master->getOriginY() != new_map->info.origin.position.y))
    {
        // 如果master costmap 层发生变化，就更新layered_costmap_ 范围
        // 并更新分层代价地图(以及所有图层，包括此图层)的大小
        // Update the size of the layered costmap (and all layers, including this one)
        ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
        layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
                                    new_map->info.origin.position.y,
                                    true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
    }
    // 方法二:若本层的数据和接收到的静态地图的参数不同时，继续以接收到的地图为准，调整本层参数。
    else if (size_x_ != size_x || size_y_ != size_y ||
             resolution_ != new_map->info.resolution ||
             origin_x_ != new_map->info.origin.position.x ||
             origin_y_ != new_map->info.origin.position.y)
    {
        // only update the size of the costmap stored locally in this layer
        // 仅更新static costmap 层信息
        ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
        resizeMap(size_x, size_y, new_map->info.resolution,
                  new_map->info.origin.position.x, new_map->info.origin.position.y);
    }
    // size_x_，size_y_是从Costmap2D类中继承过来的
    // 此时size_x_，size_y_，resolution_，origin_x_，origin_y_都已经和输入的map消息同步
    unsigned int index = 0;
    // 根据新地图中的数据来更新代价地图的值,具体转换在interpretValue函数
    // Step 3:initialize the costmap with static data 用订阅到的map数据来初始化本层static map的数据成员costmap
    for (unsigned int i = 0; i < size_y; ++i)
    {
        for (unsigned int j = 0; j < size_x; ++j)
        {
            // 地图传来的值为-1(即255),0 和100
            unsigned char value = new_map->data[index];
            costmap_[index] = interpretValue(value);
            ++index;
        }
    }
    // 获得新地图的frame_id
    map_frame_ = new_map->header.frame_id;
    // 有了新地图，更新高和宽
    // we have a new map, update full size of map
    x_ = y_ = 0;
    width_ = size_x_; // 这个是以像素坐标系(地图左下角)为参考的坐标，是int类型
    height_ = size_y_;
    map_received_ = true;
    has_updated_data_ = true;
    // 如果first_map_only_标志位是打开的，关闭map订阅
    // shutdown the map subscrber if firt_map_only_ flag is on
    if (first_map_only_)
    {
        ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
        map_sub_.shutdown();
    }
}

void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr &update)
{
    unsigned int di = 0;
    for (unsigned int y = 0; y < update->height; y++)
    {
        unsigned int index_base = (update->y + y) * size_x_;
        for (unsigned int x = 0; x < update->width; x++)
        {
            unsigned int index = index_base + x + update->x;
            costmap_[index] = interpretValue(update->data[di++]);
        }
    }
    x_ = update->x;
    y_ = update->y;
    width_ = update->width;
    height_ = update->height;
    has_updated_data_ = true;
}

void StaticLayer::activate() { onInitialize(); }

void StaticLayer::deactivate()
{
    map_sub_.shutdown();
    if (subscribe_to_updates_) map_update_sub_.shutdown();
}

void StaticLayer::reset()
{
    if (first_map_only_) has_updated_data_ = true;
    else onInitialize();
}

// 参数 min_x = min_y = 1e30，max_x = max_y = -1e30. 没有用到robot_x,robot_y,robot_yaw
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
        double *min_x, double *min_y, double *max_x, double *max_y)
{
    // Step 1: 判断是否处于滚动窗口
    if (!layered_costmap_->isRolling())
    {
        // has_extra_bounds_初始值为false，而在第一次结束前，has_updated_data_被设置为false
        // 且订阅话题在第一次接收到消息后关闭，故对于static layer，updateBounds只进行一次
        // Bounds的范围是整张地图的大小，在updateBounds过程中没有对静态地图层做任何的更新
        if (!map_received_ || !(has_updated_data_ || has_extra_bounds_)) return;
    }
    // Step 2: 使用扩展边界
    useExtraBounds(min_x, min_y, max_x, max_y);
    double wx, wy;
    // Step 3:坐标转换 计算静态costmap地图左下角像素的坐标
    mapToWorld(x_, y_, wx, wy);
    *min_x = std::min(wx, *min_x);
    *min_y = std::min(wy, *min_y);
    // 计算静态costmap地图右上角像素的坐标
    mapToWorld(x_ + width_, y_ + height_, wx, wy);
    *max_x = std::max(wx, *max_x);
    *max_y = std::max(wy, *max_y);
    has_updated_data_ = false;
}

void StaticLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
{
    if (!map_received_) return;
    if (!layered_costmap_->isRolling())
    {
        // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
        // Step 1: 如果不滚动，则分层costmap（master_grid）具有与此图层相同的坐标
        if (!use_maximum_) updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
        else updateWithMax(master_grid, min_i, min_j, max_i, max_j);
    }
    else
    {
        // If rolling window, the master_grid is unlikely to have same coordinates as this layer
        //Step 2: 如果滚动窗口，则master_grid不太可能具有与此图层相同的坐标
        unsigned int mx, my;
        double wx, wy;
        // a.Might even be in a different frame 甚至可能位于不同的框架中,需要验证坐标转换关系是否正确
        geometry_msgs::TransformStamped transform;
        try
        {
            transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
        }
        catch (tf2::TransformException ex)
        {
            ROS_ERROR("%s", ex.what());
            return;
        }
        // Copy map data given proper transformations
        // b.在进行正确转换的情况下复制地图数据
        tf2::Transform tf2_transform;
        tf2::convert(transform.transform, tf2_transform);
        for (unsigned int i = min_i; i < max_i; ++i)
        {
            for (unsigned int j = min_j; j < max_j; ++j)
            {
                // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
                // 1)将master_grid坐标(i,j)转换为global_frame_(wx,wy)坐标
                layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
                // Transform from global_frame_ to map_frame_
                // 2)将global_frame_转换成map_frame_
                tf2::Vector3 p(wx, wy, 0);
                p = tf2_transform * p;
                // Set master_grid with cell from map
                // 3) 使用地图中的单元格设置master_grid
                if (worldToMap(p.x(), p.y(), mx, my))
                {
                    if (!use_maximum_) master_grid.setCost(i, j, getCost(mx, my));
                    else
                    {
#ifdef SEMANTIC
                        master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
                        if (toXJUcost(getCost(mx, my)) > toXJUcost(master_grid.getCost(i, j)))
                        {
                            master_grid.setCost(i, j, toOri(getCost(mx, my),
                                    std::max(toXJUoption(getCost(mx, my)), toXJUoption(master_grid.getCost(i, j)))));
                        }
#else
                        master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
#endif
                    }
                }
            }
        }
    }
}

}  // namespace costmap_2d
